/*
 * PID.c
 *
 *  Created on: 10-12-2012
 *      Author: Maciej
 */


#include "Comum.h"


#define PID_MAX_INTEGRAL 600
#define PID_MAX_DERRIVAT 600

sFIR filtr;

void PID_Initialize(sPID* pid_structure, float Pp, float Ii, float Dd, float dTt)
{
	pid_structure->P = Pp;
	pid_structure->I = Ii;
	pid_structure->D = Dd;

	pid_structure->dT = dTt;

	pid_structure->last_error = 0;
	pid_structure->integral = 0;
	pid_structure->derriv = 0;

	//TODO:
	pid_structure->set = 0;

	FIR_Init(&filtr);
}

inline float CLAMP(float value, float limens)
{
	if (value > limens)
		value = limens;
	else if (value < -limens) value = -limens;

	return value;
}

float PID_Process(sPID* pid_structure, float control)
{
	float error = pid_structure->set - control;

	pid_structure->integral += error * pid_structure->dT;
	pid_structure->integral = CLAMP(pid_structure->integral, PID_MAX_INTEGRAL);

	if(pid_structure->D!=0)
	{
		pid_structure->derriv = (error - pid_structure->last_error) / pid_structure->dT;
		pid_structure->derriv = CLAMP(pid_structure->derriv, PID_MAX_DERRIVAT);
		pid_structure->derriv = FIR_Get(&filtr, pid_structure->derriv);
	}

	float ret = ( ( pid_structure->P * error) + ( pid_structure->integral * pid_structure->I) + (pid_structure->D * pid_structure->derriv));

	pid_structure->last_error = error;

	return ret;
}

void PID_Reset(sPID* pid_structure)
{
	pid_structure->last_error = 0;
	pid_structure->integral = 0;
	pid_structure->derriv = 0;
	pid_structure->set = 0;
}

void PID_Print(sPID* pid_structure)
{
	UARTprintf("PID[%4i,%4i,%4i]", (int) (100.0 * pid_structure->P), (int) (100.0 * pid_structure->I), (int) (100.0 * pid_structure->D));
}
